/*
 * user_distance_app.c
 *
 *  Created on: Mey 4, 2023
 *      Author: Administrator
 */


#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include "acc_config.h"
#include "acc_definitions_a121.h"
#include "acc_definitions_common.h"
#include "acc_hal_definitions_a121.h"
#include "acc_hal_integration_a121.h"
#include "acc_integration.h"
#include "acc_processing.h"
#include "acc_rss_a121.h"
#include "acc_sensor.h"

#include "acc_version.h"
#include "user_app.h"

#define DIST_SENSOR_ID          (1U)
#define DIST_SENSOR_TIMEOUT_MS  (1000U)
#define DIST_MAX_DATA_ENTRY_LEN 15 // "-32000+-32000i" + zero termination
A121_distance_config_t  A121_distance_config;


static void dist_set_config(acc_config_t *config);
static void dist_print_data(acc_int16_complex_t *data, uint16_t data_length);

static void dist_cleanup(acc_config_t *config, acc_processing_t *processing,
                    acc_sensor_t *sensor, void *buffer);


#define DOWN_AND_UP_COUNT_M_ST  20//连续下降和上升值 计数

#define MULTI_STEP_PEAK_NUB   10
typedef struct {
	uint16_t  peak_point[MULTI_STEP_PEAK_NUB];//峰值个数  最大5个
	uint16_t  peak_valu[MULTI_STEP_PEAK_NUB];//峰值大小  最大5个
	uint16_t  dist_valu[MULTI_STEP_PEAK_NUB];//峰值大小  最大5个
	uint8_t   peak_nub;//
}targets_peak_multi_step_t;


uint16_t  find_one_peak(uint16_t *data, uint16_t data_length)
{
	uint16_t i = 0;
	uint16_t nub=0;
	uint16_t peak = 0;
	peak=data[0];
	for(i=1;i<data_length;i++)
	{
		if(peak<data[i])
		{
			peak=data[i];
			nub=i;
		}
	}
	return nub;
}



targets_peak_multi_step_t  find_all_peak(uint16_t *data, uint16_t data_length)
{
	uint16_t i = 0;
	uint16_t peak = 0;
	uint16_t peak_count=0;//上升计数
	uint16_t trough_count=0;//下降计数

	targets_peak_multi_step_t peak_msg={0};

	for (i = 0 ; i<data_length ; i++)
	{
		if (data[i+1] > data[i] )
		{
			peak_count++;
			trough_count=0;//从新查找下降沿
		}
		else
		{
			trough_count++;
			if(trough_count>DOWN_AND_UP_COUNT_M_ST)//连续下降个数
			{
				if(peak_count>DOWN_AND_UP_COUNT_M_ST)
				{
					if(peak_msg.peak_nub<MULTI_STEP_PEAK_NUB)
					{
						peak=i-trough_count;
						if(data[peak]>(A121_distance_config.threshold))
						{
							peak_msg.peak_point[peak_msg.peak_nub]=peak;//检测到一个峰值
							peak_msg.peak_valu[peak_msg.peak_nub]=data[peak];
							peak_msg.peak_nub++;
						}
						peak_count=0;
						trough_count=0;//从新查找
					}
					else
					{
						break;
					}
				}
			}
		}
	}
	return peak_msg;
}



int user_dist_service(void)
{
	acc_config_t              *config     = NULL;
	acc_processing_t          *processing = NULL;
	acc_sensor_t              *sensor     = NULL;
	void                      *buffer     = NULL;
	uint32_t                  buffer_size = 0;
	acc_processing_metadata_t proc_meta;
	acc_processing_result_t   proc_result;

	uint32_t tickstart1;
	uint32_t tickstart2;
	uint32_t temp;

	A121_distance_config.run_status=1;
	printf("Acconeer software version %s\n", acc_version_get());


	tickstart1 = HAL_GetTick();//

	const acc_hal_a121_t *hal = acc_hal_rss_integration_get_implementation();

	if (!acc_rss_hal_register(hal))
	{
		return EXIT_FAILURE;
	}

	config = acc_config_create();
	if (config == NULL)
	{
		printf("acc_config_create() failed\n");
		dist_cleanup(config, processing, sensor, buffer);
		return EXIT_FAILURE;
	}

	reset_dist_set();
	dist_set_config(config);

	// Print the configuration
	acc_config_log(config);

	processing = acc_processing_create(config, &proc_meta);
	if (processing == NULL)
	{
		printf("acc_processing_create() failed\n");
		dist_cleanup(config, processing, sensor, buffer);
		return EXIT_FAILURE;
	}

	if (!acc_rss_get_buffer_size(config, &buffer_size))
	{
		printf("acc_rss_get_buffer_size() failed\n");
		dist_cleanup(config, processing, sensor, buffer);
		return EXIT_FAILURE;
	}

	buffer = acc_integration_mem_alloc(buffer_size);
	if (buffer == NULL)
	{
		printf("buffer allocation failed\n");
		dist_cleanup(config, processing, sensor, buffer);
		return EXIT_FAILURE;
	}

	acc_hal_integration_sensor_supply_on(DIST_SENSOR_ID);
	acc_hal_integration_sensor_enable(DIST_SENSOR_ID);

	sensor = acc_sensor_create(DIST_SENSOR_ID);
	if (sensor == NULL)
	{
		printf("acc_sensor_create() failed\n");
		dist_cleanup(config, processing, sensor, buffer);
		return EXIT_FAILURE;
	}

	bool             status;
	bool             cal_complete = false;
	acc_cal_result_t cal_result;

	do
	{
		status = acc_sensor_calibrate(sensor, &cal_complete, &cal_result, buffer, buffer_size);

		if (status && !cal_complete)
		{
			status = acc_hal_integration_wait_for_sensor_interrupt(DIST_SENSOR_ID, DIST_SENSOR_TIMEOUT_MS);
		}
	} while (status && !cal_complete);

	if (!status)
	{
		printf("acc_sensor_calibrate() failed\n");
		acc_sensor_status(sensor);
		dist_cleanup(config, processing, sensor, buffer);
		return EXIT_FAILURE;
	}

	// Reset sensor after calibration by disabling it
	acc_hal_integration_sensor_disable(DIST_SENSOR_ID);
	acc_hal_integration_sensor_enable(DIST_SENSOR_ID);




	if (!acc_sensor_prepare(sensor, config, &cal_result, buffer, buffer_size))
	{
		printf("acc_sensor_prepare() failed\n");
		acc_sensor_status(sensor);
		dist_cleanup(config, processing, sensor, buffer);
		return EXIT_FAILURE;
	}

	tickstart2 = HAL_GetTick();
	temp=tickstart2-tickstart1;

	const uint32_t update_count = 500000U;

	//for (uint32_t i = 0U; i < update_count; i++)
	while(A121_distance_config.run_status==1)
	{
		tickstart1 = HAL_GetTick();
		if (!acc_sensor_measure(sensor))
		{
			printf("acc_sensor_measure failed\n");
			acc_sensor_status(sensor);
			dist_cleanup(config, processing, sensor, buffer);
			return EXIT_FAILURE;
		}

		if (!acc_hal_integration_wait_for_sensor_interrupt(DIST_SENSOR_ID, DIST_SENSOR_TIMEOUT_MS))
		{
			printf("Sensor interrupt timeout\n");
			acc_sensor_status(sensor);
			dist_cleanup(config, processing, sensor, buffer);
			return EXIT_FAILURE;
		}

		if (!acc_sensor_read(sensor, buffer, buffer_size))
		{
			printf("acc_sensor_read failed\n");
			acc_sensor_status(sensor);
			dist_cleanup(config, processing, sensor, buffer);
			return EXIT_FAILURE;
		}

		acc_processing_execute(processing, buffer, &proc_result);
		tickstart2 = HAL_GetTick();
		temp=tickstart2-tickstart1;

		dist_print_data(proc_result.frame, proc_meta.sweep_data_length);
		HAL_Delay(A121_distance_config.data_output_time);
		A121_app_save_data();
	}

	dist_cleanup(config, processing, sensor, buffer);

	printf("Application finished OK\n");

	return EXIT_SUCCESS;
}


static void dist_cleanup(acc_config_t *config, acc_processing_t *processing,
                    acc_sensor_t *sensor, void *buffer)
{
	acc_hal_integration_sensor_disable(DIST_SENSOR_ID);
	acc_hal_integration_sensor_supply_off(DIST_SENSOR_ID);

	if (sensor != NULL)
	{
		acc_sensor_destroy(sensor);
	}

	if (processing != NULL)
	{
		acc_processing_destroy(processing);
	}

	if (config != NULL)
	{
		acc_config_destroy(config);
	}

	if (buffer != NULL)
	{
		acc_integration_mem_free(buffer);
	}
}


static void dist_print_data(acc_int16_complex_t *data, uint16_t data_length)
{
	//printf("Processed data:\n");
	char buffer[DIST_MAX_DATA_ENTRY_LEN];

	uint16_t  a121_data[data_length];
	targets_peak_multi_step_t peak_msg={0};

//	uint16_t dist[MULTI_STEP_PEAK_NUB]={0};

	uint32_t data_t=0;
	uint32_t max_data=0;
	uint16_t max_point=0;
	uint16_t dist=0;
	uint16_t i;
	float x;
	for (i = 0; i < data_length; i++)
	{
		a121_data[i]=sqrt(data[i].real*data[i].real+data[i].imag*data[i].imag);

	}

	peak_msg=find_all_peak(a121_data,data_length);

	for (i = 0; i < peak_msg.peak_nub; i++)
	{
		peak_msg.dist_valu[i]=A121_distance_config.start*1000+peak_msg.peak_point[i]*A121_distance_config.step_length*2.5;

		printf("[%d]dist=%5d,peak=%5d\n", i,peak_msg.dist_valu[i],peak_msg.peak_valu[i]);     //打印结果  8字节
	}


//	printf("\n");
}


static void dist_set_config(acc_config_t *config)
{
	uint16_t satart_point=0;
	uint16_t nub_point=0;
	float length=0;
	float MMD_nub=0;
	length=A121_distance_config.end-A121_distance_config.start;

	satart_point=(A121_distance_config.start*10000)/25;
	nub_point=(length*1000)/(A121_distance_config.step_length*2.5);


	MMD_nub=(satart_point*2.5+nub_point*A121_distance_config.step_length*2.5)/1000;

	acc_config_start_point_set(config, satart_point);
	acc_config_num_points_set(config, nub_point);// 最大4095    num_points*sweeps_per_frame<4095

	acc_config_step_length_set(config, A121_distance_config.step_length);
	acc_config_profile_set(config, A121_distance_config.profile);
	acc_config_hwaas_set(config, A121_distance_config.hwaas);
	acc_config_receiver_gain_set(config, A121_distance_config.gain);
	acc_config_sweeps_per_frame_set(config, A121_distance_config.sweeps_per_frame);
	acc_config_sweep_rate_set(config, A121_distance_config.sweep_rate);
	acc_config_continuous_sweep_mode_set(config, A121_distance_config.continuous_sweep_mode);
	acc_config_frame_rate_set(config, A121_distance_config.frame_rate);

	acc_config_inter_frame_idle_state_set(config, A121_distance_config.frame_idle);
	acc_config_inter_sweep_idle_state_set(config, A121_distance_config.sweep_idle);

	if(A121_distance_config.profile==1)
	{
		if(MMD_nub<3.1){A121_distance_config.prf=ACC_CONFIG_PRF_19_5_MHZ;}
		else if(MMD_nub<5.1){A121_distance_config.prf=ACC_CONFIG_PRF_15_6_MHZ;}
		else if(MMD_nub<7.0){A121_distance_config.prf=ACC_CONFIG_PRF_13_0_MHZ;}
		else if(MMD_nub<12.7){A121_distance_config.prf=ACC_CONFIG_PRF_8_7_MHZ;}
		else if(MMD_nub<18.5){A121_distance_config.prf=ACC_CONFIG_PRF_6_5_MHZ;}
		else if(MMD_nub<24.3){A121_distance_config.prf=ACC_CONFIG_PRF_5_2_MHZ;}
		else {A121_distance_config.prf=ACC_CONFIG_PRF_5_2_MHZ;}
	}
	else
	{
		if(MMD_nub<5.1){A121_distance_config.prf=ACC_CONFIG_PRF_15_6_MHZ;}
		else if(MMD_nub<7.0){A121_distance_config.prf=ACC_CONFIG_PRF_13_0_MHZ;}
		else if(MMD_nub<12.7){A121_distance_config.prf=ACC_CONFIG_PRF_8_7_MHZ;}
		else if(MMD_nub<18.5){A121_distance_config.prf=ACC_CONFIG_PRF_6_5_MHZ;}
		else if(MMD_nub<24.3){A121_distance_config.prf=ACC_CONFIG_PRF_5_2_MHZ;}
		else {A121_distance_config.prf=ACC_CONFIG_PRF_5_2_MHZ;}
	}

	acc_config_prf_set(config, A121_distance_config.prf);//根据用户设置的量程自动适应

	acc_config_phase_enhancement_set(config, A121_distance_config.phase_enhancemen_flag);
	acc_config_double_buffering_set(config, A121_distance_config.double_buffering_flag);


}


void reset_dist_set(void)
{
	A121_distance_config.start=0.2;
	A121_distance_config.end=3.0;


	A121_distance_config.step_length=8;
	A121_distance_config.profile=3;
	A121_distance_config.hwaas=1;
	A121_distance_config.gain=16;//1~23
	A121_distance_config.sweeps_per_frame=1;
	A121_distance_config.sweep_rate=0;
	A121_distance_config.continuous_sweep_mode=0;
	A121_distance_config.frame_rate=0;

	A121_distance_config.frame_idle=ACC_CONFIG_IDLE_STATE_DEEP_SLEEP;
	A121_distance_config.sweep_idle=ACC_CONFIG_IDLE_STATE_READY;

	A121_distance_config.prf=ACC_CONFIG_PRF_6_5_MHZ;


	A121_distance_config.phase_enhancemen_flag=0;
	A121_distance_config.double_buffering_flag=0;

	A121_distance_config.threshold=700;

}
